#include <ros/ros.h>
#include <Eigen/Dense>
#include <cmath>
#include <nav_msgs/Odometry.h>

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Quaternion.h> 
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/State.h>
// #include "prometheus_msgs/UAVCommand.h"
// #include "prometheus_msgs/UAVState.h"
#include <std_msgs/Bool.h>
// 自定义的msg，用来画曲线
//#include "prometheus_demo/MyMatrix.h"
#include "gimbal_control/rotation_matrix.h"


//定义变换矩阵等
Eigen::Vector3d t_bc,t_wb,t_ca; //平移向量
Eigen::Quaterniond q_wb,q_ca,q_bc;          //四元数
Eigen::Matrix3d rotation_matrix_bc,rotation_matrix_wb,rotation_matrix_ca; //旋转矩阵
Eigen::Matrix3d rotation_matrix_0b;  //相机初始位置转到机身坐标系


Eigen::Isometry3d Twa,Tca,Tbc,Twb,Twc;  //变换矩阵
Eigen::Isometry3d last_Twa; //上一时刻的变换矩阵

//prometheus_demo::MyMatrix plot_msg;   // 作图的自定义消息
mavros_msgs::State current_state;
bool is_detected = false;
int land_flag = -1;
Eigen::Vector3d UGV_vel;
Eigen::Vector3d UGV_vel_world;
double UGV_pos[2];
double UAV_vel[3];
double UAV_land_vel[3];




//回调函数
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
        {
            t_wb << msg->pose.position.x ,msg->pose.position.y ,msg->pose.position.z;
            //在Eigen库中,四元数的存储顺序是w、x、y、z
            q_wb = Eigen::Quaterniond(msg->pose.orientation.w ,msg->pose.orientation.x ,msg->pose.orientation.y ,msg->pose.orientation.z);
            q_wb.normalize();
            rotation_matrix_wb = q_wb.toRotationMatrix();
            //Twb.rotate( rotation_matrix_wb );
            //Twb.pretranslate( t_wb );
            Twb.linear() = rotation_matrix_wb;
            Twb.translation() = t_wb;
        }

void rotation_matrix_cb(const gimbal_control::rotation_matrix::ConstPtr &msg)
{
    t_ca << msg->translation_vector[0], msg->translation_vector[1], msg->translation_vector[2];
    rotation_matrix_ca << msg->rotation_matrix[0], msg->rotation_matrix[1], msg->rotation_matrix[2],
                           msg->rotation_matrix[3], msg->rotation_matrix[4], msg->rotation_matrix[5],
                           msg->rotation_matrix[6], msg->rotation_matrix[7], msg->rotation_matrix[8];
    Tca.linear() = rotation_matrix_ca;
    Tca.translation() = t_ca;
}

void gimbal_pos_cb(const geometry_msgs::Quaternion::ConstPtr &msg)
        {
            t_bc << 0 ,0 ,0;
            //在Eigen库中,四元数的存储顺序是w、x、y、z
            q_bc = Eigen::Quaterniond(msg->w ,msg->x ,msg->y ,msg->z);
            q_bc.normalize();
            rotation_matrix_bc = q_bc.toRotationMatrix();
            Eigen::Vector3d euler_angles = q_bc.toRotationMatrix().eulerAngles(2, 1, 0);  // ZYX顺序
            std::cout << "Euler Angles: \n" << euler_angles.transpose() << std::endl;
            Eigen::Matrix3d R_yxz =
                (Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(-euler_angles[1], Eigen::Vector3d::UnitX()) *
                Eigen::AngleAxisd(-euler_angles[2], Eigen::Vector3d::UnitZ()))
                    .matrix(); 
            std::cout << "R_yxz =\n" << R_yxz << std::endl;
            //Twb.rotate( rotation_matrix_wb );
            //Twb.pretranslate( t_wb );
            Tbc.linear() = rotation_matrix_0b * R_yxz;
            Tbc.translation() = t_bc;
        }

// void is_dete_cb(const std_msgs::Bool::ConstPtr &msg)
// {
//     if(msg->data)
//     {
//         is_detected = true;
//     }
//     else
//     {
//         is_detected = false;
//     }
// }

// void state_cb(const mavros_msgs::State::ConstPtr& msg){
//     current_state = *msg;
// }

// void UgvReceiveVEL(const geometry_msgs::Twist msg)
// {
//     UGV_vel[0] = 0;
//     UGV_vel[1] = msg.linear.x;
//     UGV_vel[2] = 0;
// }

// void UgvPos_cb(const nav_msgs::Odometry::ConstPtr& msg)
// {
//     UGV_pos[0] = msg->pose.pose.position.x;
//     UGV_pos[1] = msg->pose.pose.position.y;
// }

// void UAVStateCb(const prometheus_msgs::UAVState::ConstPtr &msg)
// {
//     UAV_vel[0] = msg->velocity[0];
//     UAV_vel[1] = msg->velocity[1];
//     UAV_vel[2] = msg->velocity[2];
// }




int main(int argc, char **argv)
{
    ros::init(argc, argv, "aruco_det");
    ros::NodeHandle nh;

    rotation_matrix_0b <<   0, 0, -1,
                            -1, 0, 0,
                            0, 1, 0; //相机初始位置转到机身坐标系

    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/typhoon_h480_0/mavros/local_position/pose", 1, pos_cb);  

    // ros::Subscriber is_dete_sub = nh.subscribe<std_msgs::Bool>("/is_detected", 1, is_dete_cb);
    // ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/uav1/mavros/state", 10, state_cb);
    // ros::Subscriber UGV_VEL_sub = nh.subscribe<geometry_msgs::Twist>("/cmd_vel",10,UgvReceiveVEL);
    // ros::Subscriber UGV_POS_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/landing_pad",10,UgvPos_cb);
    // ros::Subscriber UAV_state_sub = nh.subscribe<prometheus_msgs::UAVState>("/uav1/prometheus/state", 10, UAVStateCb);

    ros::Subscriber rotation_matrix_sub = nh.subscribe<gimbal_control::rotation_matrix>("/rotation_matrix", 10, rotation_matrix_cb);
    //ros::Publisher plot_pub = nh.advertise<prometheus_demo::MyMatrix>("/uav1/MyMatrix", 10);

    ros::Subscriber gimbal_pose_sub = nh.subscribe<geometry_msgs::Quaternion>("/typhoon_h480_0/mavros/mount_control/orientation",10,gimbal_pos_cb);

    ros::Publisher local_setpoint_pub = nh.advertise<geometry_msgs::Pose>("/xtdrone/typhoon_h480_0/cmd_pose_enu", 1);

    //固定相机的Tbc    
    // t_bc <<  0, 0, 0;
    // rotation_matrix_bc << 0, -1, 0,
    //                      -1 , 0, 0,
    //                       0, 0, -1;
    // Tbc.linear() = rotation_matrix_bc;
    // Tbc.translation() = t_bc;

    // prometheus_msgs::UAVCommand g_command_now;
    // DynamicPositionController controller(nh);
    int timer_flag =0;
    ros::Time last_request = ros::Time::now();
    ros::Rate rate(50.0);

    while(ros::ok())
    {
        ros::spinOnce();
        Twa = Twb * Tbc * Tca ;
                std::cout << "Twb translation vector: \n" << Twb.translation() << std::endl;
                std::cout << "Tbc translation vector: \n" << Tbc.translation() << std::endl;
                std::cout << "Tca translation vector: \n" << Tca.translation() << std::endl;
                std::cout << "Twa translation vector: \n" << Twa.translation() << std::endl;
                std::cout << "Twb matrix: \n" << Twb.matrix() << std::endl;
                std::cout << "Tbc matrix: \n" << Tbc.matrix() << std::endl;
                std::cout << "Tca matrix: \n" << Tca.matrix() << std::endl;
                std::cout << "Twa matrix: \n" << Twa.matrix() << std::endl;
                std::cout << "Aruco in World error: " << Twa.translation() - Twb.translation() <<std::endl;
        
        rate.sleep();
    }

    return 0;
}